/**
 * @file RobotControl.c
 * @author jjm (2411241788@qq.com)
 * @brief 2024-底盘（运动解算+速度+路径规划）
 * @version
 * @date 2024-03-26
 *
 *
 *@新添功能；沿自身坐标系X轴直线前进，
 * 沿自身坐标系Y轴直线前进，角度环实现自转
 *@日期: 2025-4-3
 *
 */
/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/
#include "RobotControl.h"
#include "math.h"

/*
*********************************************************************************************************
*                                               CONSTANTS
*********************************************************************************************************
*/
enum v_type{
  x_v = 0,
  y_v
};

typedef struct plus_v{
  float Target_point_velocity;//pass pos
  float Next_point_velocity;//hitting ball pos
}plus_v;

plus_v my_expect_plus_v = {0,0};


const double robot_length = 0.650f;//
const double robot_width = 0.650f;
const double Rotate_R = 0.3325;  //
#define Speed_Limit 2.0
#define Mutation_Limit 1.0
#define Speed_Limit_min 0.1
#define Initial_Speed_Limit 1.0
#define Add_Speed 0.1
#define Recify_Limit 0.3
#define Acceleration_Limit 0.7
#define Jerk_Limit 0.7
#define Distance_Error 0.2
#define Distance_Error_y 0.25
#define Distance_recify_Error 0.03
#define Circularity_Error 0.05
Robot_INFO Robot;


/****************跑直线时使用的位置结构体********************/
Position Self_Original_Position_Y = {0, 0, 0};/*自身坐标下Y_X,Y_Y,Y_YAW*/
Position Self_Original_Position_X = {0, 0, 0};/*自身坐标下X_X,X_Y,X_YAW*/
Position World_Original_Position_Y = {0, 0, 0};/*世界坐标下Y_X,Y_Y,Y_YAW*/
Position World_Original_Position_X = {0, 0, 0};/*世界坐标下X_X,X_Y,X_YAW*/
/*********************************************************/
#define MAX_ACCEL_UP_VX   0.8f     // X方向加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VX 100.0f   // X方向减速时最大加速度 (绝对值减小)
#define MAX_ACCEL_UP_VY   0.8f     // Y方向加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VY 100.0f   // Y方向减速时最大加速度 (绝对值减小)
#define MAX_ACCEL_UP_VW   1.0f     // 角速度加速时最大加速度 (绝对值增大)
#define MAX_ACCEL_DOWN_VW 100.5f   // 角速度减速时最大加速度 (绝对值减小)

Position Original_Position_XY = {0, 0, 0};
Position Original_Position_Distance_Plan = {0, 0, 0};
Position End_Position_Distance_Plan = {0, 0, 0};
Position Speed_Control = {0, 0, 0};
Position Zero = {0, 0, 0};
float World_x = 0, World_y = 0, World_yaw = 0;
float temp = 0;
float Two_Speed_Abs = 0;
float Speed_Angle = 0;

auto_flag auto_sate_flag = {0};

/******************************************************/
/*位置环pid:分3段*/

/*第一阶段pid,此时从起点出发*/
PIDType pid_error_x_close = {0.8, 3.5, 0.1, 0, 0, 0, 0, 0};
PIDType pid_error_y_close = {0.8, 3.5, 0.1, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_close= {0.9, 8, 0.16, 0, 0, 0, 0, 0};
/*第二阶段pid,此时车在路途上*/
PIDType pid_error_x_middle = {0.7, 2.5, 0.1, 0, 0, 0, 0, 0};
PIDType pid_error_y_middle = {0.7,2.5, 0.1, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_middle= {0.9, 6.5, 0.16, 0, 0, 0, 0, 0};
/*第三阶段pid,此时靠近目标位置*/
PIDType pid_error_x = {0.35, 1, 0.05, 0, 0, 0, 0, 0};
PIDType pid_error_y = {0.35, 1, 0.05, 0, 0, 0, 0, 0};
PIDType pid_error_yaw = {0.25, 4, 0.08, 0, 0, 0, 0, 0};
unsigned char PID_position_flag = 0;
unsigned char PID_position_flag_pass_through = 0;
float error_limit = 0.022f;
extern unsigned char error_flag;
/******************************************************/

/*****************专门用于回边界的pid******************/
/*所有的值变为原来的2倍*/
/*第三阶段pid,此时靠近目标位置*/
PIDType pid_error_x_goback_close = {1.6, 5, 0.2, 0, 0, 0, 0, 0};
PIDType pid_error_y_goback_close = {1.6, 5, 0.2, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_goback_close= {1.8,12, 0.08, 0, 0, 0, 0, 0};
/*第二阶段pid,此时车在路途上*/
PIDType pid_error_x_goback_middle = {1.4, 3.5, 0.2, 0, 0, 0, 0, 0};
PIDType pid_error_y_goback_middle = {1.4, 3.5, 0.2, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_goback_middle= {1.8, 9, 0.16, 0, 0, 0, 0, 0};
/*第一阶段pid,此时从起点出发*/
PIDType pid_error_goback_x = {1.2, 1.8,0.10, 0, 0, 0, 0, 0};
PIDType pid_error_goback_y = {1.2, 1.8, 0.10, 0, 0, 0, 0, 0};
PIDType pid_error_goback_yaw = {1.8, 12, 0.16, 0, 0, 0, 0, 0};
unsigned char PID_position_flag_goback = 0;
extern uint8_t index;
/******************************************************/
/*角度环pid*/
PIDType pid_error_yaw_x = {1.25, 2, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_y = {1.25, 2, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_yaw = {1.35,2,1.25, 0, 0, 0, 0};
unsigned char PID_yaw_flag = 0;
/*激光位置环*/
PIDType pid_error_x_by_lader = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_y_by_lader = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_yaw_by_lader = {0.45, 3, 0.10, 0, 0, 0, 0, 0};
static unsigned char PID_position_flag_by_lader = 0;
/*****************************************************/

/****************YYX于2025-4-11添加的******************/
/*沿X自身坐标系x轴走直线的pid*/
PIDType pid_error_in_x_self_x = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_x_self_y = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_x_self_yaw = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
unsigned char PID_X_in_self_flag = 0;/*初始化标志位*/
/*沿自身坐标系y轴走直线的pid*/
PIDType pid_error_in_y_self_x = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_y_self_y = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_y_self_yaw = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
unsigned char PID_Y_in_self_flag = 0;/*初始化标志位*/
/*沿世界坐标系x轴走直线的pid*/
PIDType pid_error_in_x_world_x = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_x_world_y = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_x_world_yaw = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
unsigned char PID_X_in_world_flag = 0;/*初始化标志位*/
/*沿世界坐标系y轴走直线的pid*/
PIDType pid_error_in_y_world_x = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_y_world_y = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
PIDType pid_error_in_y_world_yaw = {0.35, 1, 0.02, 0, 0, 0, 0, 0};
unsigned char PID_Y_in_world_flag = 0;/*初始化标志位*/
/******************************************************/

float flag_yaw =1.0;
/*****码盘传来的数据结构体*****/
extern AS5047_raw_t AS_5047;

/**
 * @brief 舵轮底盘解算函数
 *
 * @param[in] none
 *
 * @return none
 *
 * @note 用于运动模型解析
 * Wheel No. 1 is located in front left
 * the rest are arranged counterclockwise
 */
/*
*********************************************************************************************************
*                                            	变量定义
*********************************************************************************************************
*/
//Wheel No. 1 is located in front left
int Nav_i;
float OMG[WHEELNUM + 1] = {0, 0.25 * PI, 0.25 * PI, 0.25 * PI, 0.25 * PI};
float Wheel_Now_Direction[WHEELNUM + 1] = {0};
float Wheel_Last_Direction[WHEELNUM + 1] = {0, 0, 0, 0};
float Wheel_Front_Direction[WHEELNUM + 1] = {0, 0, 0, 0};

float turnFlag[WHEELNUM + 1] = {1, 1, 1, 1,1};
float Vx_part[WHEELNUM + 1] = {0, 0, 0, 0};
float Vy_part[WHEELNUM + 1] = {0, 0, 0, 0};
//uint8_t step = 0;
uint8_t step_1 = 0;
int speed_flag = 0;
// int flag_1 = 0;
float Passed_Distance = 0.0;
float omega = 0;
Robot_INFO Robot;
#define MAX_ACCEL_VX 0.5f   // X方向最大加速度 (m/s²)
#define MAX_ACCEL_VY 0.5f   // Y方向最大加速度
#define MAX_ACCEL_VW 1.0f   // 旋转方向最大角加速度 (rad/s²)
/*
*********************************************************************************************************
*                                               函数实现
*********************************************************************************************************
*/

/**
 * @brief 机器人速度转轮速
 * @param none
 * @return None
 * @note 机器人在自身坐标系下速度转到轮速
 */
void Robot_Velocity_To_Wheel_Speed() // 速度分解
{
  float Vx, Vy, Vw;
  
  // 获取机器人在自身坐标系下的期望速度  (交换Vx和Vy)
  Vy = Robot.Robot_in_self.velocity_exp.Vx;
  Vx = Robot.Robot_in_self.velocity_exp.Vy;
  Vw = Robot.Robot_in_self.velocity_exp.Vw * Rotate_R; // 车轮到三角中心的距离
  
  // 运动参数分解
  // 计算第一个轮子的速度
  Robot.Wheel_Speed[2] = (sqrt(2)/2) * Vx + (sqrt(2)/2) * Vy + Vw * Rotate_R;
  
  // 计算第二个轮子的速度
  Robot.Wheel_Speed[3] = (sqrt(2)/2) * Vx - (sqrt(2)/2) * Vy + Vw * Rotate_R;
  
  // 计算第三个轮子的速度
  Robot.Wheel_Speed[4] = -(sqrt(2)/2) * Vx - (sqrt(2)/2) * Vy + Vw * Rotate_R;
  
  // 计算第四个轮子的速度
  Robot.Wheel_Speed[1] = -(sqrt(2)/2) * Vx + (sqrt(2)/2) * Vy + Vw * Rotate_R;
  
}
/**
 * @brief 轮速转电机
 * @param none
 * @return None
 *
 */

void Wheel_Speed_To_Motor()
{
  for (int i = 1; i <= WHEELNUM; i++)
  {
    M3508[i].ExpSpeed = (int16_t)(60*19*Robot.Wheel_Speed[i] / (2 * PI * WHEEL_R));
  }
}
/**
 * @brief 机器人世界坐标系转自身坐标系速度
 * @param none
 * @return None
 */
void Robot_Velocity_World_To_Self()
{
  float Vx, Vy, Vw;
  Vx = Robot.Robot_in_world.velocity_exp.Vx;
  Vy = Robot.Robot_in_world.velocity_exp.Vy;
  Vw = Robot.Robot_in_world.velocity_exp.Vw;
  Robot.Robot_in_self.velocity_exp.Vx = Vx * cos(Robot.Robot_in_self.position_now.yaw) + Vy * sin(Robot.Robot_in_self.position_now.yaw);
  Robot.Robot_in_self.velocity_exp.Vy = -Vx * sin(Robot.Robot_in_self.position_now.yaw) + Vy * cos(Robot.Robot_in_self.position_now.yaw);
  Robot.Robot_in_self.velocity_exp.Vw = Vw;
}

/**
 * @brief 控制机器人移动到指定位置和姿态（PID控制）
 * @param expect_x 目标X坐标（单位：米）
 * @param expect_y 目标Y坐标（单位：米）
 * @param expect_yaw 目标偏航角（单位：弧度）
 * @note 该函数使用PID控制器计算速度指令，并限制最大速度
 */
//void Robot_Move_To_Target_Position(float expect_x, float expect_y, float expect_yaw)
//{
//    PIDType *px, *py, *pyaw;
//    /* 第一次调用时设置目标位置 */
//    if (PID_position_flag == 0)
//    {  
//        Robot.Robot_in_self.position_exp.x = expect_x;
//        Robot.Robot_in_self.position_exp.y = expect_y;
//        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
//        PID_position_flag = 1;
//    }
//    
//    /* 计算位置误差 */
//    
//    /****将码盘的数据进行赋给robot*/
//
//    /*****************************/
//    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
//    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
//    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
//    if(fabs(error_x) < 0.25 && fabs(error_y) < 0.25 && fabs(error_yaw) < 0.25) {
//        px = &pid_error_x_close;
//        py = &pid_error_y_close;
//        pyaw = &pid_error_yaw_close;
//    } else if(0.25 < fabs(error_x) < 1 && 0.25 <fabs(error_y) < 1 && 0.25 <fabs(error_yaw) < 0.9) {
//        px = &pid_error_x_middle;
//        py = &pid_error_y_middle;
//        pyaw = &pid_error_yaw_middle;
//    }
//    else{
//        px = &pid_error_x;
//        py = &pid_error_y;
//        pyaw = &pid_error_yaw;
//    }
//    /* PID计算期望速度 */
//    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px, error_x);
//    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py, error_y);
//    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw, error_yaw);
//    
//    /* 速度限幅处理 */
//    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
//    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
//    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);
//    /*the error of hiting mode and throwing mode is different
//    *when Auto_mode is throw mode,the error flag is set 0;
//    *when Auto mode is hitting mode ,the error flag is set 1;
//    */
//    if(error_flag == 0){error_limit = 0.03f;}
//    if(error_flag == 1){error_limit = 0.15f;}
//   /* 到达目标位置判断（误差小于0.1） */
//    if ((fabs(error_x) < 0.03) && (fabs(error_y) < 0.03) && (fabs(error_yaw) < 0.03))
//    {//0.022f
//        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
//        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
//        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
//        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
//        PID_position_flag = 0;  // 重置初始化标志
//        
//        if(!auto_sate_flag.auto_wants_ones)
//        {
//          auto_sate_flag.arrival = 1;
//          auto_sate_flag.auto_wants_ones = 1;
//        }
//    }
//}

void Robot_Move_To_Target_Position_goback(float expect_x, float expect_y, float expect_yaw)
{
    float limit_yaw;
    PIDType *px, *py, *pyaw;                             //临时变量暂存pid模型
    static float prev_Vx = 0, prev_Vy = 0, prev_Vw = 0;  // 保存上一周期速度
    static uint32_t last_time = 0;                       // 时间戳记录,上一次时间
    uint32_t current_time = 0;                           // 当前时间
    float dt = 0;                                        //dt = currnet_time - last_time,用以计算加速度
    float delta_Vx= 0,delta_Vy = 0,delta_Vw = 0; 
    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0; 
    /* 第一次调用时设置目标位置 */
    if (PID_position_flag_goback == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_position_flag_goback = 1;
    }
    /***********三段Pid实现位置环***********/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
    /*******根据当前误差确定使用哪一阶段的pid*******/
    /*******第一阶段pid(Kp,ki,kd取不同的值)*********/
    if(fabs(error_x) < 0.25 && fabs(error_y) < 0.25 && fabs(error_yaw) < 0.25) {
        px = &pid_error_x_goback_close;
        py = &pid_error_y_goback_close;
        pyaw = &pid_error_yaw_goback_close;
    } 
    /*******第二段pid*******/
    else if(0.25 < fabs(error_x) < 1 && 0.25 <fabs(error_y) < 1 && 0.25 <fabs(error_yaw) < 0.9) {
        px = &pid_error_x_goback_middle;
        py = &pid_error_y_goback_middle;
        pyaw = &pid_error_yaw_goback_middle;
    }
    /******第三段pid*******/
    else{
        px = &pid_error_goback_x;
        py = &pid_error_goback_y;
        pyaw = &pid_error_goback_yaw;
    }
    /* PID计算期望速度 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px, error_x);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py, error_y);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.0);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.0);
    if(index >= 3 && index <= 6){limit_yaw = 2.5;}
    if(index < 3){limit_yaw = 6;}
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, limit_yaw);
    
     /* 加速度限制 */
    /*****************************************************************************************************************/
    current_time = HAL_GetTick();
    /***得到时间差***/
    dt = (current_time - last_time) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;
    last_time = current_time;
    /*X方向加速度限幅*/
    delta_Vx = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx;
    /*通过绝对值判断是加速还是减速，如果大于表明机器人此时加速否则为减速*/
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx)) {max_delta_Vx = 0.5 * dt;} //加速时求得最大速度改变量      
    else { max_delta_Vx = MAX_ACCEL_DOWN_VX * dt;}                                                                //减速时求得最大速度改变量
    delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);                                               //确定最终速度改变量（加速度限幅）
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx + delta_Vx;                                             //再次确定期望速度
    /*Y方向加速度限幅*/
    delta_Vy = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy)){max_delta_Vy = 0.5 * dt;} 
    else {max_delta_Vy = MAX_ACCEL_DOWN_VY * dt;}
    delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy + delta_Vy;
    /*角加速度限幅*/
    delta_Vw = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw)) { max_delta_Vw = MAX_ACCEL_UP_VW * dt;} 
    else {max_delta_Vw = MAX_ACCEL_DOWN_VW * dt;}
    delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw + delta_Vw;
    /******************************************************************************************************************/
    /*二次速度限幅*/
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.0);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.0);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, limit_yaw);

    /* 更新历史速度，用于下次加速度限幅 */
    prev_Vx = Robot.Robot_in_world.velocity_exp.Vx;   
    prev_Vy = Robot.Robot_in_world.velocity_exp.Vy;
    prev_Vw = Robot.Robot_in_world.velocity_exp.Vw;
    /*the error of hiting mode and throwing mode is different
    *when Auto_mode is throw mode,the error flag is set 0;
    *when Auto mode is hitting mode ,the error flag is set 1;
    */
    if(error_flag == 0){error_limit = 0.03f;}
    if(error_flag == 1){error_limit = 0.15f;}
   /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.30) && (fabs(error_y) < 0.30) && (fabs(error_yaw) < 0.25))
    {//0.022f
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_position_flag_goback = 0;  // 重置初始化标志
        
        if(!auto_sate_flag.auto_wants_ones)
        {
          auto_sate_flag.arrival = 1;
          auto_sate_flag.auto_wants_ones = 1;
        }
    }


}

void Robot_Move_To_Target_Position_dingdian(float expect_x, float expect_y, float expect_yaw)
{
    PIDType *px, *py, *pyaw;                             //临时变量暂存pid模型
    static float prev_Vx = 0, prev_Vy = 0, prev_Vw = 0;  // 保存上一周期速度
    static uint32_t last_time = 0;                       // 时间戳记录,上一次时间
    uint32_t current_time = 0;                           // 当前时间
    float dt = 0;                                        //dt = currnet_time - last_time,用以计算加速度
    float delta_Vx= 0,delta_Vy = 0,delta_Vw = 0; 
    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0; 
    /*******第一次调用时设置目标位置*******/
    if (PID_position_flag == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_position_flag = 1;
    }
    /***********三段Pid实现位置环***********/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
    /*******根据当前误差确定使用哪一阶段的pid*******/
    /*******第一阶段pid(Kp,ki,kd取不同的值)*********/
    if(fabs(error_x) < 0.25 && fabs(error_y) < 0.25 && fabs(error_yaw) < 0.25) 
    {
        px = &pid_error_x_close;
        py = &pid_error_y_close;
        pyaw = &pid_error_yaw_close;
    } 
    /*******第二段pid*******/
    else if(0.25 < fabs(error_x) < 1 && 0.25 <fabs(error_y) < 1 && 0.25 <fabs(error_yaw) < 0.9) 
    {
        px = &pid_error_x_middle;
        py = &pid_error_y_middle;
        pyaw = &pid_error_yaw_middle;
    }
    /******第三段pid*******/
    else
    {
        px = &pid_error_x;
        py = &pid_error_y;
        pyaw = &pid_error_yaw;
    }
    /* PID计算期望速度 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px, error_x);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py, error_y);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 3);
    
     /* 加速度限制 */
    /*****************************************************************************************************************/
    current_time = HAL_GetTick();
    /***得到时间差***/
    dt = (current_time - last_time) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;
    last_time = current_time;
    /*X方向加速度限幅*/
    delta_Vx = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx;
    /*通过绝对值判断是加速还是减速，如果大于表明机器人此时加速否则为减速*/
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx)) {max_delta_Vx = MAX_ACCEL_UP_VX * dt;} //加速时求得最大速度改变量      
    else { max_delta_Vx = MAX_ACCEL_DOWN_VX * dt;}                                                                //减速时求得最大速度改变量
    delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);                                               //确定最终速度改变量（加速度限幅）
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx + delta_Vx;                                             //再次确定期望速度
    /*Y方向加速度限幅*/
    delta_Vy = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy)){max_delta_Vy = MAX_ACCEL_UP_VY * dt;} 
    else {max_delta_Vy = MAX_ACCEL_DOWN_VY * dt;}
    delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy + delta_Vy;
    /*角加速度限幅*/
    delta_Vw = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw)) { max_delta_Vw = MAX_ACCEL_UP_VW * dt;} 
    else {max_delta_Vw = MAX_ACCEL_DOWN_VW * dt;}
    delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw + delta_Vw;
    /******************************************************************************************************************/
    /*二次速度限幅*/
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);

    /* 更新历史速度，用于下次加速度限幅 */
    prev_Vx = Robot.Robot_in_world.velocity_exp.Vx;   
    prev_Vy = Robot.Robot_in_world.velocity_exp.Vy;
    prev_Vw = Robot.Robot_in_world.velocity_exp.Vw;
    /*the error of hiting mode and throwing mode is different
    *when Auto_mode is throw mode,the error flag is set 0;
    *when Auto mode is hitting mode ,the error flag is set 1;
    */
    if(error_flag == 0){error_limit = 0.03f;}
    if(error_flag == 1){error_limit = 0.15f;}
   /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.005) && (fabs(error_y) < 0.005) && (fabs(error_yaw) < 0.005))
    {//0.022f
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_position_flag = 0;  // 重置初始化标志
        
        if(!auto_sate_flag.auto_wants_ones)
        {
          auto_sate_flag.arrival = 1;
          auto_sate_flag.auto_wants_ones = 1;
        }
    }
}



void Robot_Move_To_Target_Position(float expect_x, float expect_y, float expect_yaw)
{
    PIDType *px, *py, *pyaw;                             //临时变量暂存pid模型
    static float prev_Vx = 0, prev_Vy = 0, prev_Vw = 0;  // 保存上一周期速度
    static uint32_t last_time = 0;                       // 时间戳记录,上一次时间
    uint32_t current_time = 0;                           // 当前时间
    float dt = 0;                                        //dt = currnet_time - last_time,用以计算加速度
    float delta_Vx= 0,delta_Vy = 0,delta_Vw = 0; 
    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0; 
    /* 第一次调用时设置目标位置 */
    if (PID_position_flag == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_position_flag = 1;
    }
    /***********三段Pid实现位置环***********/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
    /*******根据当前误差确定使用哪一阶段的pid*******/
    /*******第一阶段pid(Kp,ki,kd取不同的值)*********/
    if(fabs(error_x) < 0.25 && fabs(error_y) < 0.25 && fabs(error_yaw) < 0.25) {
        px = &pid_error_x_close;
        py = &pid_error_y_close;
        pyaw = &pid_error_yaw_close;
    } 
    /*******第二段pid*******/
    else if(0.25 < fabs(error_x) < 1 && 0.25 <fabs(error_y) < 1 && 0.25 <fabs(error_yaw) < 0.9) {
        px = &pid_error_x_middle;
        py = &pid_error_y_middle;
        pyaw = &pid_error_yaw_middle;
    }
    /******第三段pid*******/
    else{
        px = &pid_error_x;
        py = &pid_error_y;
        pyaw = &pid_error_yaw;
    }
    /* PID计算期望速度 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px, error_x);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py, error_y);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 4);
    
     /* 加速度限制 */
    /*****************************************************************************************************************/
    current_time = HAL_GetTick();
    /***得到时间差***/
    dt = (current_time - last_time) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;
    last_time = current_time;
    /*X方向加速度限幅*/
    delta_Vx = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx;
    /*通过绝对值判断是加速还是减速，如果大于表明机器人此时加速否则为减速*/
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx)) {max_delta_Vx = MAX_ACCEL_UP_VX * dt;} //加速时求得最大速度改变量      
    else { max_delta_Vx = MAX_ACCEL_DOWN_VX * dt;}                                                                //减速时求得最大速度改变量
    delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);                                               //确定最终速度改变量（加速度限幅）
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx + delta_Vx;                                             //再次确定期望速度
    /*Y方向加速度限幅*/
    delta_Vy = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy)){max_delta_Vy = MAX_ACCEL_UP_VY * dt;} 
    else {max_delta_Vy = MAX_ACCEL_DOWN_VY * dt;}
    delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy + delta_Vy;
    /*角加速度限幅*/
    delta_Vw = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw)) { max_delta_Vw = MAX_ACCEL_UP_VW * dt;} 
    else {max_delta_Vw = MAX_ACCEL_DOWN_VW * dt;}
    delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw + delta_Vw;
    /******************************************************************************************************************/
    /*二次速度限幅*/
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.5);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.5);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 4);

    /* 更新历史速度，用于下次加速度限幅 */
    prev_Vx = Robot.Robot_in_world.velocity_exp.Vx;   
    prev_Vy = Robot.Robot_in_world.velocity_exp.Vy;
    prev_Vw = Robot.Robot_in_world.velocity_exp.Vw;
    /*the error of hiting mode and throwing mode is different
    *when Auto_mode is throw mode,the error flag is set 0;
    *when Auto mode is hitting mode ,the error flag is set 1;
    */
    if(error_flag == 0){error_limit = 0.03f;}
    if(error_flag == 1){error_limit = 0.15f;}
   /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.01) && (fabs(error_y) < 0.01) && (fabs(error_yaw) < 0.01))
    {//0.022f
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_position_flag = 0;  // 重置初始化标志
       
    }
}

float velocity_plus(float expect_x, float expect_y, float exp_plus_v, int x_or_y ){
  // x_or_y = 0 --> x    x_or_y = 1 --> y
  float plus_x_or_y_velocity = 0;
  float delta_x = expect_x - Robot.Robot_in_self.position_now.x;
  float delta_y = expect_y - Robot.Robot_in_self.position_now.y;
  float angle_r = atan2f(delta_y, delta_x);
  
  if(x_or_y == 0){
    plus_x_or_y_velocity = exp_plus_v * cos(angle_r);
  }
  else{
    plus_x_or_y_velocity = exp_plus_v * sin(angle_r);
  }
  
  plus_x_or_y_velocity = Limit_Speed(plus_x_or_y_velocity, 1.5);
  
  return plus_x_or_y_velocity;
    
}


void Robot_Move_To_Target_Position_pass_through(float expect_x, float expect_y, float expect_yaw)
{
    PIDType *px1, *py1, *pyaw1;                             //临时变量暂存pid模型
    static float prev_Vx1 = 0, prev_Vy1 = 0, prev_Vw1 = 0;  // 保存上一周期速度
    static uint32_t last_time1 = 0;                       // 时间戳记录,上一次时间
    uint32_t current_time = 0;                           // 当前时间
    float dt = 0;                                        //dt = currnet_time - last_time,用以计算加速度
    float delta_Vx= 0,delta_Vy = 0,delta_Vw = 0; 
    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0; 
    /* 第一次调用时设置目标位置 */
    if (PID_position_flag_pass_through == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_position_flag_pass_through = 1;
    }
    
    /* 计算位置误差 */
    
    /****将码盘的数据进行赋给robot*/

    /*****************************/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
    /*****************************/
    
    /*******第一阶段pid(Kp,ki,kd取不同的值)*********/
    if(fabs(error_x) < 0.3 && fabs(error_y) < 0.3) {
        px1 = &pid_error_x_close;
        py1 = &pid_error_y_close;
        pyaw1 = &pid_error_yaw_close;
        
        my_expect_plus_v.Target_point_velocity = 0.25;
        my_expect_plus_v.Next_point_velocity = 0.8;
    } 
    /**********************************************/
        /*******第二段pid*******/
    else if(0.3 < fabs(error_x) < 1 && 0.3 <fabs(error_y) < 1) {
        px1 = &pid_error_x_middle;
        py1 = &pid_error_y_middle;
        pyaw1 = &pid_error_yaw_middle;
        my_expect_plus_v.Target_point_velocity = 0.75;
        my_expect_plus_v.Next_point_velocity = 0;
        
    }
    /**********************************************/
    /******第三段pid*******/
    else{
        px1 = &pid_error_x;
        py1 = &pid_error_y;
        pyaw1 = &pid_error_yaw;
        
        my_expect_plus_v.Target_point_velocity = 0.75;
        my_expect_plus_v.Next_point_velocity = 0;
    }
    
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px1, error_x) 
      + velocity_plus(expect_x, expect_y, my_expect_plus_v.Target_point_velocity, x_v)
      + velocity_plus(hitting_x_point[index], hitting_y_point[index], my_expect_plus_v.Next_point_velocity, x_v);
    
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py1, error_y)
      + velocity_plus(expect_x, expect_y, my_expect_plus_v.Target_point_velocity, y_v)
      + velocity_plus(hitting_x_point[index], hitting_y_point[index], my_expect_plus_v.Next_point_velocity, y_v);
    
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw1, error_yaw);
    /*********************************************/
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.7);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.7);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);
     /* 加速度限制 */
    /*****************************************************************************************************************/
    current_time = HAL_GetTick();
    /***得到时间差***/
    dt = (current_time - last_time1) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;
    last_time1 = current_time;
    /*X方向加速度限幅*/
    delta_Vx = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx1;
    /*通过绝对值判断是加速还是减速，如果大于表明机器人此时加速否则为减速*/
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx1)) {max_delta_Vx = MAX_ACCEL_UP_VX * dt;} //加速时求得最大速度改变量      
    else { max_delta_Vx = MAX_ACCEL_DOWN_VX * dt;}                                                                //减速时求得最大速度改变量
    delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);                                               //确定最终速度改变量（加速度限幅）
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx1 + delta_Vx;                                             //再次确定期望速度
    /*Y方向加速度限幅*/
    delta_Vy = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy1;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy1)){max_delta_Vy = MAX_ACCEL_UP_VY * dt;} 
    else {max_delta_Vy = MAX_ACCEL_DOWN_VY * dt;}
    delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy1 + delta_Vy;
    /*角加速度限幅*/
    delta_Vw = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw1;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw1)) { max_delta_Vw = MAX_ACCEL_UP_VW * dt;} 
    else {max_delta_Vw = MAX_ACCEL_DOWN_VW * dt;}
    delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw1 + delta_Vw;
    /******************************************************************************************************************/
    /*二次速度限幅*/
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);
    
    /* 更新历史速度，用于下次加速度限幅 */
    prev_Vx1 = Robot.Robot_in_world.velocity_exp.Vx;   
    prev_Vy1 = Robot.Robot_in_world.velocity_exp.Vy;
    prev_Vw1 = Robot.Robot_in_world.velocity_exp.Vw;
    /*the error of hiting mode and throwing mode is different
    *when Auto_mode is throw mode,the error flag is set 0;
    *when Auto mode is hitting mode ,the error flag is set 1;
    */
    if(error_flag == 0){error_limit = 0.03f;}
    if(error_flag == 1){error_limit = 0.15f;}
   /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.15f) && 
        (fabs(error_y) < 0.15f))
    {
        
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_position_flag_pass_through = 0;  // 重置初始化标志
        
        auto_sate_flag.pass_through = 1;
        
    }
  
}

void Robot_Move_To_Target_Position_hitting_ball(float expect_x, float expect_y, float expect_yaw)
{
    PIDType *px, *py, *pyaw;                             //临时变量暂存pid模型
    static float prev_Vx = 0, prev_Vy = 0, prev_Vw = 0;  // 保存上一周期速度
    static uint32_t last_time = 0;                       // 时间戳记录,上一次时间
    uint32_t current_time = 0;                           // 当前时间
    float dt = 0;                                        //dt = currnet_time - last_time,用以计算加速度
    float delta_Vx= 0,delta_Vy = 0,delta_Vw = 0; 
    float max_delta_Vx = 0,max_delta_Vy = 0,max_delta_Vw = 0; 
    /* 第一次调用时设置目标位置 */
    if (PID_position_flag == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_position_flag = 1;
    }
    /***********三段Pid实现位置环***********/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;
    /*******根据当前误差确定使用哪一阶段的pid*******/
    /*******第一阶段pid(Kp,ki,kd取不同的值)*********/
    if(fabs(error_x) < 0.25 && fabs(error_y) < 0.25 && fabs(error_yaw) < 0.25) {
        px = &pid_error_x_close;
        py = &pid_error_y_close;
        pyaw = &pid_error_yaw_close;
    } 
    /*******第二段pid*******/
    else if(0.25 < fabs(error_x) < 1 && 0.25 <fabs(error_y) < 1 && 0.25 <fabs(error_yaw) < 0.9) {
        px = &pid_error_x_middle;
        py = &pid_error_y_middle;
        pyaw = &pid_error_yaw_middle;
    }
    /******第三段pid*******/
    else{
        px = &pid_error_x;
        py = &pid_error_y;
        pyaw = &pid_error_yaw;
    }
    /* PID计算期望速度 */
    my_expect_plus_v.Target_point_velocity = 0.32;//期望加上的速度
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(px, error_x) + velocity_plus(expect_x, expect_y, my_expect_plus_v.Target_point_velocity, x_v);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(py, error_y) + velocity_plus(expect_x, expect_y, my_expect_plus_v.Target_point_velocity, y_v);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = -PIDCal(pyaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);
    
     /* 加速度限制 */
    /*****************************************************************************************************************/
    current_time = HAL_GetTick();
    /***得到时间差***/
    dt = (current_time - last_time) / 1000.0f;
    dt = (dt < 0.001f) ? 0.001f : dt;
    last_time = current_time;
    /*X方向加速度限幅*/
    delta_Vx = Robot.Robot_in_world.velocity_exp_Unsure.Vx - prev_Vx;
    /*通过绝对值判断是加速还是减速，如果大于表明机器人此时加速否则为减速*/
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vx) > fabs(prev_Vx)) {max_delta_Vx = MAX_ACCEL_UP_VX * dt;} //加速时求得最大速度改变量      
    else { max_delta_Vx = MAX_ACCEL_DOWN_VX * dt;}                                                                //减速时求得最大速度改变量
    delta_Vx = fmaxf(fminf(delta_Vx, max_delta_Vx), -max_delta_Vx);                                               //确定最终速度改变量（加速度限幅）
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = prev_Vx + delta_Vx;                                             //再次确定期望速度
    /*Y方向加速度限幅*/
    delta_Vy = Robot.Robot_in_world.velocity_exp_Unsure.Vy - prev_Vy;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vy) > fabs(prev_Vy)){max_delta_Vy = MAX_ACCEL_UP_VY * dt;} 
    else {max_delta_Vy = MAX_ACCEL_DOWN_VY * dt;}
    delta_Vy = fmaxf(fminf(delta_Vy, max_delta_Vy), -max_delta_Vy);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = prev_Vy + delta_Vy;
    /*角加速度限幅*/
    delta_Vw = Robot.Robot_in_world.velocity_exp_Unsure.Vw - prev_Vw;
    if (fabs(Robot.Robot_in_world.velocity_exp_Unsure.Vw) > fabs(prev_Vw)) { max_delta_Vw = MAX_ACCEL_UP_VW * dt;} 
    else {max_delta_Vw = MAX_ACCEL_DOWN_VW * dt;}
    delta_Vw = fmaxf(fminf(delta_Vw, max_delta_Vw), -max_delta_Vw);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = prev_Vw + delta_Vw;
    /******************************************************************************************************************/
    /*二次速度限幅*/
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 1.5);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 1.5);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);

    /* 更新历史速度，用于下次加速度限幅 */
    prev_Vx = Robot.Robot_in_world.velocity_exp.Vx;   
    prev_Vy = Robot.Robot_in_world.velocity_exp.Vy;
    prev_Vw = Robot.Robot_in_world.velocity_exp.Vw;
    /*the error of hiting mode and throwing mode is different
    *when Auto_mode is throw mode,the error flag is set 0;
    *when Auto mode is hitting mode ,the error flag is set 1;
    */
    if(error_flag == 0){error_limit = 0.03f;}
    if(error_flag == 1){error_limit = 0.15f;}
   /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.08) && (fabs(error_y) < 0.08))
    {//0.022f
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_position_flag = 0;  // 重置初始化标志
        
        if(!auto_sate_flag.auto_wants_ones)
        {
          auto_sate_flag.arrival = 1;
          auto_sate_flag.auto_wants_ones = 1;
        }
    }
}

/**
 * @brief 控制机器人移动到目标姿态yaw（PID控制）
 * @param expect_yaw 目标偏航角（单位：弧度）
 * @note 该函数使用PID控制器计算速度指令，并限制最大速度
 */
void Robot_Yaw_To_Target_Position(float expect_yaw)
{
    static unsigned char PID_yaw_flag= 0;  // 初始化标志位
    
    /* 第一次调用时设置目标位置 */
    if (PID_yaw_flag == 0)
    {  
        Robot.Robot_in_self.position_exp.x = Robot.Robot_in_self.position_now.x;
        Robot.Robot_in_self.position_exp.y = Robot.Robot_in_self.position_now.y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        PID_yaw_flag = 1;
    }
    
    /* 计算位置误差 */
    
    /****将码盘的数据进行赋给robot*/

    /*****************************/
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw - Robot.Robot_in_self.position_now.yaw;

    /* PID计算期望速度 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(&pid_error_yaw_x, error_x);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(&pid_error_yaw_y, error_y);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw += -PIDCal(&pid_error_yaw_yaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.5);
    Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.5);
    Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 2.5);
    
    /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.005f) && 
        (fabs(error_y) < 0.005f) && 
        (fabs(error_yaw) < 0.05f))
    {
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        Robot.Robot_in_world.velocity_exp_Unsure.Vw = 0.0f;
        PID_yaw_flag = 0;  // 重置初始化标志
        
    }
}

/**
 * @brief 限制速度大小
 * @param speed 输入速度
 * @param limit 速度限制值
 * @return 限制后的速度
 */
float Limit_Speed(float speed, float limit)
{
    if (fabs(speed) > limit)
    {
        return limit * (speed > 0 ? 1.0f : -1.0f);
    }
    return speed;
}


/*
*@brief:机器人沿自身坐标系X轴走直线,并保持底盘不转动
*@date: 2025-4-3
*
*/
void Robot_Move_Self_PID_X(float Posion_x)
{
  if (PID_X_in_self_flag == 0)
  {
    Self_Original_Position_X.x = Robot.Robot_in_self.position_now.x;
    Self_Original_Position_X.y = Robot.Robot_in_self.position_now.y;
    Self_Original_Position_X.yaw = Robot.Robot_in_self.position_now.yaw;
    PID_X_in_self_flag = 1;
  }
  float error_x = Posion_x + Self_Original_Position_X.x - Robot.Robot_in_self.position_now.x;
  float error_y = Self_Original_Position_X.y - Robot.Robot_in_self.position_now.y;
  float error_w = Self_Original_Position_X.yaw - Robot.Robot_in_self.position_now.yaw;
  /*位置环调节*/
  Robot.Robot_in_self.velocity_exp_Unsure.Vx = PIDCal(&pid_error_in_x_self_x, error_x);
  Robot.Robot_in_self.velocity_exp_Unsure.Vy = PIDCal(&pid_error_in_x_self_y, error_y);
  Robot.Robot_in_self.velocity_exp_Unsure.Vw = PIDCal(&pid_error_in_x_self_yaw, error_w);
  /*速度Vx,Vy,Vw限幅*/
  Robot.Robot_in_self.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vx, 2.2);
  Robot.Robot_in_self.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vy, 2.2);
  Robot.Robot_in_self.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vw, 0.5);
  /*误差判断*/
  if (fabs(error_x) < Distance_Error && fabs(error_y) < Distance_recify_Error && fabs(error_w) < Distance_recify_Error)
  {
    Robot.Robot_in_self.velocity_exp.Vx = 0;
    Robot.Robot_in_self.velocity_exp.Vy = 0;
    Robot.Robot_in_self.velocity_exp.Vw = 0;
    PID_X_in_self_flag = 0;
    step++;
  }
}


/*
*@brief:机器人沿自身坐标系Y轴走直线,并保持底盘不转动
*@date: 2025-4-3
*
*/
void Robot_Move_Self_PID_Y(float Posion_y)
{
  if (PID_Y_in_self_flag == 0)
  {
    Self_Original_Position_Y.x = Robot.Robot_in_self.position_now.x;
    Self_Original_Position_Y.y = Robot.Robot_in_self.position_now.y;
    Self_Original_Position_Y.yaw = Robot.Robot_in_self.position_now.yaw;
    PID_Y_in_self_flag = 1;
  }
  float error_y = Posion_y + Self_Original_Position_Y.y - Robot.Robot_in_self.position_now.y;
  float error_x = Self_Original_Position_Y.x - Robot.Robot_in_self.position_now.x;
  float error_w = Self_Original_Position_Y.yaw - Robot.Robot_in_self.position_now.yaw;
  /*位置环调节*/
  Robot.Robot_in_self.velocity_exp_Unsure.Vx = PIDCal(&pid_error_in_y_self_x,error_x);
  Robot.Robot_in_self.velocity_exp_Unsure.Vy = PIDCal(&pid_error_in_y_self_y,error_y);
  Robot.Robot_in_self.velocity_exp_Unsure.Vw = PIDCal(&pid_error_in_y_self_yaw,error_w);
  /*速度Vx,Vy,Vw限幅*/
  Robot.Robot_in_self.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vx, 2.2);
  Robot.Robot_in_self.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vy, 2.2);
  Robot.Robot_in_self.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_self.velocity_exp_Unsure.Vw, 0.5);
  /*误差判断*/
  if (fabs(error_y) < Distance_Error_y && fabs(error_x) < Distance_recify_Error && fabs(error_w) < Distance_recify_Error)
  {
    Robot.Robot_in_self.velocity_exp.Vx = 0;
    Robot.Robot_in_self.velocity_exp.Vy = 0;
    Robot.Robot_in_self.velocity_exp.Vw = 0;
    PID_Y_in_self_flag = 0;
    step++;
  }
}


/*
*@brief:机器人沿世界坐标系X轴走直线,并保持底盘不转动
*@date: 2025-4-3
*@author:
*/
void Robot_Move_World_PID_X(float Posion_x)
{
  if (PID_X_in_world_flag == 0)
  {
    World_Original_Position_X.x = Robot.Robot_in_world.position_now.x;
    World_Original_Position_X.y = Robot.Robot_in_world.position_now.y;
    World_Original_Position_X.yaw = Robot.Robot_in_world.position_now.yaw;
    PID_X_in_world_flag = 1;
  }
  float error_x = Posion_x + World_Original_Position_X.x - Robot.Robot_in_world.position_now.x;
  float error_y = World_Original_Position_X.y - Robot.Robot_in_world.position_now.y;
  float error_w = World_Original_Position_X.yaw - Robot.Robot_in_world.position_now.yaw;
  
  Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(&pid_error_in_x_world_x,error_x);
  Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(&pid_error_in_x_world_y, error_y);
  Robot.Robot_in_world.velocity_exp_Unsure.Vw = PIDCal(&pid_error_in_x_world_yaw,error_w);
  
  Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.2);
  Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.2);
  Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 0.5);

  if (fabs(error_x) < Distance_Error && fabs(error_y) < Distance_recify_Error && fabs(error_w) < Distance_recify_Error)
  {
    Robot.Robot_in_world.velocity_exp.Vx = 0;
    Robot.Robot_in_world.velocity_exp.Vy = 0;
    Robot.Robot_in_world.velocity_exp.Vw = 0;
    PID_X_in_world_flag = 0;
    step++;
  }
}


/*
*@brief:机器人沿世界标系Y轴走直线,并保持底盘不转动
*@date: 2025-4-3
*@author:YYX
*/
void Robot_Move_World_PID_Y(float Posion_y)
{
  if (PID_Y_in_world_flag == 0)
  {
    World_Original_Position_Y.x = Robot.Robot_in_world.position_now.x;
    World_Original_Position_Y.y = Robot.Robot_in_world.position_now.y;
    World_Original_Position_Y.yaw = Robot.Robot_in_world.position_now.yaw;
    PID_Y_in_world_flag = 1;
  }
  float error_y = Posion_y + World_Original_Position_Y.y - Robot.Robot_in_world.position_now.y;
  float error_x = World_Original_Position_Y.x - Robot.Robot_in_world.position_now.x;
  float error_w = World_Original_Position_Y.yaw - Robot.Robot_in_world.position_now.yaw;

  Robot.Robot_in_world.velocity_exp_Unsure.Vx = PIDCal(&pid_error_in_y_world_x, error_x);
  Robot.Robot_in_world.velocity_exp_Unsure.Vy = PIDCal(&pid_error_in_y_world_y, error_y);
  Robot.Robot_in_world.velocity_exp_Unsure.Vw = PIDCal(&pid_error_in_x_world_yaw, error_w);
  
  Robot.Robot_in_world.velocity_exp.Vx = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vx, 2.2);
  Robot.Robot_in_world.velocity_exp.Vy = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vy, 2.2);
  Robot.Robot_in_world.velocity_exp.Vw = Limit_Speed(Robot.Robot_in_world.velocity_exp_Unsure.Vw, 0.5);
  if (fabs(error_y) < Distance_Error_y && fabs(error_x) < Distance_recify_Error && fabs(error_w) < Distance_recify_Error)
  {
    Robot.Robot_in_world.velocity_exp.Vx = 0;
    Robot.Robot_in_world.velocity_exp.Vy = 0;
    Robot.Robot_in_world.velocity_exp.Vw = 0;
    PID_Y_in_world_flag = 0;
    step++;
  }
}


int initial_Speed_Plan = 0;
Position Original_Position_Plan = {0, 0, 0};
PIDType S_Speed_Plan_Recify_w = {0.3, 2.0, 0.02, 0, 0, 0.05, 0, 0};
float omega_x = 0;
float omega_y = 0;
void S_Speed_Plan_XY(float distance_x, float distance_y, float DeadArea)
{
  float Passed_Distance_x, Passed_Distance_y, speed_x, speed_y;
  float raising_dist_x = 0.1 * fabs(distance_x);
  float raising_dist_y = 0.1 * fabs(distance_y);
  
  float Speed_Limit_X = (fabs(distance_x) > 2.0) ? 2.0 : (fabs(distance_x) < 0.1) ? 0.1 : fabs(distance_x);
  float Speed_Limit_Y = (fabs(distance_y) > 2.0) ? 2.0 : (fabs(distance_y) < 0.1) ? 0.1 : fabs(distance_y);

  if(initial_Speed_Plan == 0) {
    Original_Position_Plan = Robot.Robot_in_self.position_now;
    initial_Speed_Plan = 1;

    omega_x = PI / raising_dist_x;  
    omega_y = PI / raising_dist_y;
  }

  double error_w = Robot.Robot_in_self.position_now.yaw - Original_Position_Plan.yaw;
  Robot.Robot_in_self.velocity_exp.Vw = PIDCal(&S_Speed_Plan_Recify_w, error_w);
  if(Robot.Robot_in_self.velocity_exp.Vw > Recify_Limit) Robot.Robot_in_self.velocity_exp.Vw = Recify_Limit;
  else if(Robot.Robot_in_self.velocity_exp.Vw < -Recify_Limit) Robot.Robot_in_self.velocity_exp.Vw = -Recify_Limit;

  Passed_Distance_x = fabs(Robot.Robot_in_self.position_now.x - Original_Position_Plan.x);
  if(Passed_Distance_x <= raising_dist_x) { 
    speed_x = Speed_Limit_X * (1 - cos(omega_x * Passed_Distance_x));
  } 
  else if(Passed_Distance_x > (fabs(distance_x) - raising_dist_x)) { 
    speed_x = Speed_Limit_X * (1 - cos(omega_x * (fabs(distance_x)-Passed_Distance_x)));
  } 
  else { 
    speed_x = Speed_Limit_X;
  }
  if(speed_x < 0.1) speed_x = 0.1;

  Passed_Distance_y = fabs(Robot.Robot_in_self.position_now.y - Original_Position_Plan.y);
  if(Passed_Distance_y <= raising_dist_y) {
    speed_y = Speed_Limit_Y * (1 - cos(omega_y * Passed_Distance_y));
  } 
  else if(Passed_Distance_y > (fabs(distance_y)-raising_dist_y)) {
    speed_y = Speed_Limit_Y * (1 - cos(omega_y * (fabs(distance_y)-Passed_Distance_y)));
  } 
  else {
    speed_y = Speed_Limit_Y;
  }
  if(speed_y < 0.1) speed_y = 0.1;

  Robot.Robot_in_self.velocity_exp.Vx = (distance_x >= 0) ? speed_x : -speed_x;
  Robot.Robot_in_self.velocity_exp.Vy = (distance_y >= 0) ? speed_y : -speed_y;

  if( (fabs(Passed_Distance_x - fabs(distance_x)) < DeadArea) &&
      (fabs(Passed_Distance_y - fabs(distance_y)) < DeadArea) ) 
  {
    Robot.Robot_in_self.velocity_exp.Vx = Robot.Robot_in_self.velocity_exp.Vy = Robot.Robot_in_self.velocity_exp.Vw = 0;
    initial_Speed_Plan = 0; 
  }
}